#include <unistd.h>
#include <math.h>

#include "base.h"
#include "motion.h"

static int const s = 50;

base::base(void)
{
	oiStart();
	setSafeMode();
	readDist();
	readAngle();
}

base::~base(void)
{
	setDrive(0,0);
}

// drive d mm at velocity v in mm/s if robot hits obstical stop motors
int base::safeDrive(short v, short d)
{
	int dTrue = 0;
	setDrive(v, v);

	// drive until distance is reached or wall is hit
	// continue to update state variables
	int c = hitWall();
	while(!c && (d > 0)?(dTrue < d):(dTrue > d)) {
		int disp = readDist();
		x += disp*cos(M_PI*theta/180.);
		y += disp*sin(M_PI*theta/180.);

		theta += readAngle();
		dTrue += disp;
		c = hitWall();
	}

	setDrive(0, 0);
	theta += readAngle();
	int disp = readDist();
	x += disp*cos(M_PI*theta/180.);
	y += disp*sin(M_PI*theta/180.);
	return c;
}

int base::safeTurn(short v, short a)
{
	int aTrue = 0;

	if(a > 0)
		setDrive(-v, v);
	else if(a < 0)
		setDrive(v, -v);
	else
		return 0;

	int c = hitWall();
	while (!c && (a > 0)?(aTrue < a):(aTrue > a)) {
		int disp = readDist();
		int angle = readAngle();
		x += disp*cos(M_PI*theta/180.);
		y += disp*sin(M_PI*theta/180.);

		aTrue += angle;
		theta += angle;
	}

	setDrive(0, 0);
	theta += readAngle();
	return c;
}

// Move robot to point <xf, yf> based on first readings
void base::moveIl(double xf, double yf, short v)
{
	double xc = xf-x;
	double yc = yf-y;
	double mag = sqrt(xc*xc + yc*yc);
	double a = 180.*-atan2(yc,xc)/M_PI;

	safeTurn(v, (short)a);
	safeDrive(v, (short)mag);
}

// Move robot to point <xf, yf> correcting along the way in steps
void base::moveSl(double xf, double yf, short v, int steps)
{
	for(int i=0; i < steps; i++) {
		double xc = xf-x;
		double yc = yf-y;
		double mag = sqrt(xc*xc + yc*yc);
		double a = 180.*-atan2(yc,xc)/M_PI;

		safeTurn(v, (short)a);
		safeDrive(v, (short)mag/steps);
	}
}

// Move toward point <xf, yf> until event returns non zero @ v
// make corrections without stopping
void base::moveTo(double xf, double yf, short v, int(*done)(void))
{
	double d=9001;
	while(!done() && d > 50) {
		double xc = xf-x;
		double yc = yf-y;
		double a = 180.*atan2(yc,xc)/M_PI; // angle to turn to be looking at <xf, yf>

		double aTrue = theta;
		// calculate shortest turning angle

		a = (fabs(aTrue-a) > 180) ? 360+a:a;
		a = a - round(a) + ((int)a+180)%360 - 180;
		printf("%f ", a);

		int ma,disp;
		if(aTrue < a && fabs(aTrue-a) > 45) {
			setDrive(-v, v);
			while(aTrue < a) {
				ma = readAngle();
				aTrue += ma;
				theta += ma;
			}
		}
		else if(aTrue > a && fabs(aTrue-a) > 45) {
			setDrive(v, -v);
			while(aTrue > a) {
				ma = readAngle();
				aTrue += ma;
				theta += ma;
			}
		}

		setDrive(v, v);
		d=sqrt(pow(x-xf,2)+pow(y-yf,2));
		disp = readDist();
		theta += readAngle();
		x += disp*cos(M_PI*theta/180.);
		y += disp*sin(M_PI*theta/180.);

		theta = theta - round(theta) + ((int)theta+180)%360 - 180;

		printState();
	}

	setDrive(0, 0);
}

void base::printState(void)
{
	printf("%8.1f %8.1f, %8.1f\n", x, y, theta);
}

state_v base::getState(void)
{
	state_v ret = {x,y,theta};

	return ret;
}
